#include <Servo.h>

/* Analog output pin */
int pin10 = 10; // movement x
int pin11 = 11; // movement y

//0 = none, 1 = robot_x, 2 = robot_y, 3 = camera_x 4 = camera_y
byte active_header;

byte prev_robot_x;
byte prev_robot_y;

//TODO
int bPS;  // Bits per segment: (segment = values that control a specific axis)
int charUsage;  // Char Usage: number of numbers in a char that are used to send data
int numSPC;  // Number of Segments per Character: usually 4
int xCenter, xRange, xOffset;
int yCenter, yRange, yOffset;
int midp;
int dead;
int stopmove;

void setup(){

	//opens serial port, sets data rate to 9600 bps
	Serial.begin(9600);

	//make the pins outputs
	pinMode(pin10, OUTPUT);
	pinMode(pin11, OUTPUT);

	prev_robot_x = 127;
	prev_robot_y = 127;

	active_header = 0;


  //TODO
  bPS = 40;
  midp = (bPS / 2) - 1;
  dead = bPS / 20;
  xCenter = 153;
  yCenter = 160;
  charUsage = 159;
  
  numSPC = bPS/charUsage;
  xRange = (255 - xCenter) * 2;
  yRange = (255 - yCenter) * 2;
  xOffset = 255 - xRange;
  yOffset = 255 - yRange;

  stopmove = 0;

}

void loop(){

	byte in_value;
	byte write_value_x;
	byte write_value_y;

	if (Serial.available() > 0) {
		//Serial.available() returns the number of bytes (characters) available for reading from the serial port. This is data that's already arrived and stored in the serial receive buffer (which holds 128 bytes). 

		//read a byte
		in_value = Serial.read();

		if(active_header == 0){
			//this is a header
			active_header = in_value;
/*
                                        Serial.print("New header:");
                                        Serial.print(active_header,DEC);
                                        Serial.print("\n");
                                        */
		}else{
	
			//this is a value corresponding to the active header
			switch(active_header){
				case 1:
					//robot_x
					prev_robot_x = in_value;
				        
/*
                                        Serial.print("Arduino robot_x:");
                                        Serial.print(prev_robot_x,DEC);
                                        Serial.print("\n");
*/                                      
                                        
                                        //TODO
                                        prev_robot_x = (in_value * xRange / bPS) + xOffset;
                                        if ((in_value > (midp - dead)) && (in_value < (midp + dead))) {
                                          prev_robot_x = xCenter;
                                        }
                                        
                                        
                                        analogWrite(pin10, prev_robot_x);  // Write a movement x value
					break;
				case 2:
					//robot_y
					prev_robot_y = in_value;
                                        /*
                                        Serial.print("Arduino robot_y:");
                                        Serial.print(prev_robot_y,DEC);
                                        Serial.print("\n");
                                        */
                                        
                                        //TODO
                                        prev_robot_y = ((in_value - 1*bPS) * yRange / bPS) + yOffset;
                                        if ((in_value > (midp - dead)) && (in_value < (midp + dead))) {
                                          prev_robot_y = yCenter;
                                        }
                                        
					analogWrite(pin11, prev_robot_y);  // Write a movement y value
					break;
				case 3:
					//camera_x
					break;
				case 4:
					//camera_y
					break;
				default:
					break;
			}

			//next time we get a new header
			active_header = 0;
		}
	}else{
		//no new info
		//if the last command was not stationary, keep doing it
		if(prev_robot_x != 127){
  /*
  Serial.print("Repeat last arduino robot_x:");
                                        Serial.print(prev_robot_x,DEC);
                                        Serial.print("\n");
                                        */
			analogWrite(pin10, prev_robot_x);
		}else if(prev_robot_y != 127){
                                          /*
                                        Serial.print("Repeat last arduino robot_y:");
                                        Serial.print(prev_robot_y,DEC);
                                        Serial.print("\n");
                                        */
			analogWrite(pin11, prev_robot_y);
		}
	}
}
